#include "imageloadingthread.h"
#include <QImage>

#include <QDebug>

ImageLoadingThread::ImageLoadingThread(const QStringList &imagePathList)
{

    m_ImgPathList = imagePathList;
    m_LeftOffset = 1;
    pk = new kin;
    pslam = new slam_app;
    m_RightOffset = 1;
}

ImageLoadingThread::~ImageLoadingThread()

{
}

void ImageLoadingThread::start(ImageLoadingThread::Direction direction)

{

    // 下面的这个if语句（起等待作用）很重要，可以自己注释后品味一下

    // if(this->isRunning()){

    //    qDebug() << "IsRunning";

    //    this->wait();

    //}

    // if (m_LeftOffset + m_RightOffset - 1 == m_ImgPathList.size())
    // {

    //     return;
    // }

    m_CurrentDirection = direction;

    // if (m_ImgCurrentIndex - m_LeftOffset < 0)
    // {

    //     m_CurrentDirection = Right;
    // }

    // if (m_ImgCurrentIndex + m_RightOffset >= m_ImgPathList.size())
    // {

    //     m_CurrentDirection = Left;
    // }

    QThread::start();
}

void ImageLoadingThread::setOriginalImageIndex(const int imgIndex)

{

    m_ImgCurrentIndex = imgIndex;
}
QImage mat2qimg(Mat &srcImage)
{
    QImage qimg;

    int width = srcImage.cols;
    int height = srcImage.rows;
    cvtColor(srcImage, srcImage, COLOR_BGR2RGB);
    qimg = QImage(srcImage.data, width, height,
                  QImage::Format_RGB888);
    return qimg;
}
#include <fstream>
void ImageLoadingThread::run(void)

{

    std::fstream fss("../2023_05_09_16_03_33/file.txt", std::ios::in);
    // cv::FileStorage fs(" ../2023_05_09_16_05_02/pose.yml", cv::FileStorage::READ);
    // cv::Mat idx;
    // fs["idx"] >> idx;
    // std::cout << idx << std::endl;
    for (int i = 0; fss.good(); ++i)
    {
        string s1;
        fss >> s1;
        if (s1.size() > 0)
            imgpaths.push_back("../2023_05_09_16_03_33" + s1);
        else
            break;
    }
    cout << imgpaths.size() << endl;
    // imgpaths.pop_back();
    fss.close();

    qInfo() << "ImageLoadingThread::run, " << m_CurrentDirection;

    for (int j = 0; j<imgpaths.size() /2; ++j)
    {

        cv::Mat inimg = cv::imread(imgpaths[j * 2 + 1]);
        cv::Mat in_image = inimg.clone();
        cv::Mat in_depth = cv::imread(imgpaths[j * 2], -1);
        assert(!in_depth.empty());
        assert(!inimg.empty());
        pslam->run(in_image, in_depth);
        if (pslam->curr_pose.empty())
            continue;
        cout << pslam->curr_pose << endl;
        pk->run(inimg, in_depth, cv::Affine3f(pslam->curr_pose));
        emit image(1, mat2qimg(pk->vdep).scaled(480, 320));
        emit image(2, mat2qimg(pk->rdp).scaled(480, 320));
        emit image(0, mat2qimg(inimg).scaled(480, 320));
    }

    // if (Left == m_CurrentDirection)
    // {

    //     QImage img(m_ImgPathList.at(m_ImgCurrentIndex - m_LeftOffset));

    //     ++m_LeftOffset;

    //     // return;
    // }

    // if (Right == m_CurrentDirection)
    // {

    //     QImage img(m_ImgPathList.at(m_ImgCurrentIndex + m_RightOffset));

    //     emit image(m_ImgCurrentIndex + m_RightOffset, img);

    //     ++m_RightOffset;

    //     // return;
    // }
}
